# coding: utf8

import RPi.GPIO as GPIO
import time


class Navigator(object):
    def __init__(self, in1, in2, in3):
        self.in3 = in3
        self.in2 = in2
        self.in1 = in1
        self.setup()

    MIN_DUTY = 2.2
    MAX_DUTY = 11.4
    CENTRE = MIN_DUTY + (MAX_DUTY - MIN_DUTY) / 2
    p_serve = None

    def setup(self):
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(self.in1, GPIO.OUT, initial=False)
        GPIO.setup(self.in2, GPIO.IN)
        GPIO.setup(self.in3, GPIO.OUT, initial=False)
        self.p_serve = GPIO.PWM(self.in3, 50)
        self.p_serve.start(self.CENTRE)
        time.sleep(0.5)
        self.p_serve.ChangeDutyCycle(0)

    def forward(self):
        self.p_serve.ChangeDutyCycle(self.CENTRE)
        time.sleep(0.5)
        self.p_serve.ChangeDutyCycle(0)

    def turn_left(self):
        self.p_serve.ChangeDutyCycle(self.MAX_DUTY)
        time.sleep(0.5)
        self.p_serve.ChangeDutyCycle(0)

    def turn_right(self):
        self.p_serve.ChangeDutyCycle(self.MIN_DUTY)
        time.sleep(0.5)
        self.p_serve.ChangeDutyCycle(0)

    def turn(self, angle):
        self.p_serve.ChangeDutyCycle(self.MIN_DUTY + (angle * (self.MAX_DUTY - self.MIN_DUTY) / 180))
        time.sleep(0.2)
        self.p_serve.ChangeDutyCycle(0)

    def setDuty(self, duty):
        self.p_serve.ChangeDutyCycle(duty)
        time.sleep(0.5)
        self.p_serve.ChangeDutyCycle(0)

    def check_front(self):
        self.turn(45)
        distances = [self.test()]

        self.turn(60)
        distances.append(self.test())

        self.turn(75)
        distances.append(self.test())

        self.turn(90)
        distances.append(self.test())

        self.turn(105)
        distances.append(self.test())

        self.turn(120)
        distances.append(self.test())

        self.turn(135)
        distances.append(self.test())

        return min(distances)

    def check_left(self):
        self.turn_left()
        return self.test()

    def check_right(self):
        self.turn_right()
        return self.test()

    def test(self):
        GPIO.output(self.in1, GPIO.HIGH)
        time.sleep(0.000015)
        GPIO.output(self.in1, GPIO.LOW)

        while not GPIO.input(self.in2):
            pass

        t1 = time.time()
        while GPIO.input(self.in2):
            pass
        t2 = time.time()
        dis = (t2 - t1) * 340 / 2

        return dis
